/*
 * message_queue.c
 *
 *  Created on: 2017-4-6
 *      Author: chenshisheng
 */

#include "message_queue.h"
#include "package_buffer.h"

#include <string.h>

/// 消息队列缓存大小（字节）
#define BUFFER_SIZE MAX(MSGQUEUE_MSG_MAX_SIZE * 10, 1024 * 2)

// 默认的消息处理函数
static Callback_t _globalNotify;

// 消息队列缓存
static uint8_t _buf[BUFFER_SIZE];

// 消息队列结构
static pbuf_t _pbuf;

#ifdef CMSIS_OS
static osSemaphoreId_t _sem;
#endif

static int _DefaultNotify(void *arg);

void MsgQueue_Init(Callback_t globalNotify)
{
    _globalNotify = globalNotify ? globalNotify : _DefaultNotify;
#ifdef CMSIS_OS
    _sem = osSemaphoreNew(10, 0, NULL);
#endif

    PackageBuffer_Init(& _pbuf, _buf, sizeof(_buf), MSGQUEUE_MSG_MAX_SIZE);
}

int MsgQueue_Send(MsgQueue_Id_t id, const void *dataIn, unsigned int size)
{
    int ret;

    OS_ENTER_CRITICAL();
    ret = PackageBuffer_Write(& _pbuf, & id, sizeof(id));
    if(ret == 0)
    {
        ret = PackageBuffer_Write(& _pbuf, dataIn, size);
    }
    OS_EXIT_CRITICAL();
#ifdef CMSIS_OS
    if(ret == 0)
    {
        ret = osSemaphoreRelease(_sem);
    }
#endif
    return ret;
}

int MsgQueue_Recv(MsgQueue_Msg_t *msgOut)
{
    int ret;
    uint8_t buf[MSGQUEUE_MSG_MAX_SIZE];
    unsigned int size;

#ifdef CMSIS_OS
    ret = osSemaphoreAcquire(_sem, osWaitForever);
    if(ret != osOK)
    {
        return ret;
    }
#endif
    OS_ENTER_CRITICAL();
    ret = PackageBuffer_Read(& _pbuf, buf, & size);
    if((ret == 0) && (size == sizeof(msgOut->id)))
    {
        memcpy(& msgOut->id, buf, sizeof(msgOut->id));
        ret = PackageBuffer_Read(& _pbuf, msgOut->data, & msgOut->size);
    }
    else
    {
        ret = -1;
    }
    OS_EXIT_CRITICAL();
    return ret;
}

static int _DefaultNotify(void *arg)
{
    ON_RELEASE(UNUSED(arg);)
    ON_DEBUG(MsgQueue_Msg_t *msg = arg;)

    debug("Msg id = %d, size = %u\n", msg->id, msg->size);
    return 0;
}

void MsgQueue_Service(void)
{
    MsgQueue_Msg_t msg;

    while(MsgQueue_Recv(& msg) == 0)
    {
        _globalNotify(& msg);
    }
}
